
import time 
import math
from threading import Thread 

from Brain.actuators.wheels import Wheels
from Brain.sensors.encoder import Encoder
from Brain.sensors.encoder_calculation import EncoderData
from Brain.movement import MotorCortex
from Brain.vector import Vector
from Brain.recording import mark_begining
from Brain.plan import MovementStep


pi_2, pi__2, = math.pi / 2.0, - math.pi / 2.0

t_s = 0.0
def change_state(encoder):
	global t_s
	encoder._process_input( EncoderData(16, -27, t_s) )
	#encoder._process_input( EncoderData(-27, 16, t_s) )
	t_s += 1
	

if __name__ == '__main__':
	encoder = Encoder()
	encoder.setDaemon(True)
	move = MotorCortex(encoder)
	
	print 'STOPING ..'
	encoder.reset(Vector(0, 0), 0)
	move.stop()
	
	move.start_task( MovementStep(0.0, -250.0) )
	
	if 0:
		new = move._read_sensors()
		if new:
			move._rotate_to_direction(new)
			print '\n\n'
			print move.history.data[-1]
			print 'done.\n\n\n'

	for x in range(30):
		move._run_logic()
		change_state(encoder)
		print move.history.data[-1]

	print 'done.'



